/**********************************************************
**             Email:@qq.com   QQ:1069841355
**---------------------------------------------------------
**  Description: 此文件为 四足机器人 控制器头 文件
**  Version    : 
**  Notes      : 
**  Author     : 于宪元
**********************************************************/
#ifndef _CONTROLLER_H_
#define _CONTROLLER_H_

#include <stdbool.h>

#include "easyMat.h"
#include "webotsInterface.h"

//-----------------------------------------------------------macro
#define max(a,b) ( ((a)>(b)) ? (a):(b) )
#define min(a,b) ( ((a)>(b)) ? (b):(a) )

#define g        (9.8)
//-----------------------------------------------------------typedef
/*
3维向量
*/
typedef struct
{
  double x;
  double y;
  double z;
}vect3TypeDef;
/*
机器人定义,包括机器人状态和机器人参数
*/
typedef struct
{
  //------------------------------------------------------------------------控制参数
  double Tf     ;  // 0.5;
  double Ts     ;  // 0.5;
  double hd     ;  // 0.6;
  double zh     ;  // -0.5;
  double Kr     ;  // 2000;
  double Kdr    ;  // 120;
  double Kp     ;  // 10000;
  double Kdp    ;  // 800;
  double Kh     ;  // 8000;
  double Kdh    ;  // 800;
  double Kvx    ;  // 1000;
  double Kwz    ;  // 1000;
  double Kvy    ;  // 0.05;
  double kx     ;  // 100;
  double kdx    ;  // 100;
  double ky     ;  // 8000
  double kdy    ;  // 100;
  double kz1    ;  // 10000;
  double kz2    ;  // 1000;
  double kdz    ;  // 100;
  double tes[4] ;  //toe_err_sum
  double I      ;
   
  double vxd_default ;  // 0.1;  //默认前向期望速度
  double wzd_default ;  // 10;   //默认转弯速度
  double vxd ;  // 0;
  double vyd ;  // 0;
  double wzd ;  // 0;
  //------------------------------------------------------------------------模型参数
  double l  ;  // 0.45;   //前后横滚髋关节距离的一半
  double w  ;  // 0.175;  //左右横滚髋关节距离的一半
  double h  ;  // 0.1;    //躯干质心与横滚髋关节垂直距离
  double a0 ;  // 0.05;   //髋部连杆长度
  double a1 ;  // 0.3;    //大腿连杆长度
  double a2 ;  // 0.3;    //小腿连杆长度
  double m0 ;  // 2;      //髋部连杆质量
  double m1 ;  // 4;      //大腿连杆质量
  double m2 ;  // 4;      //小腿连杆质量
  double M  ;  // 100;    //躯干质量
  //------------------------------------------------------------------------状态区
  double            t              ;  // 0;      //从一步开始，经过的时间
  legNameTypeDef    Fst            ;  // FL;     //前支撑腿
  legNameTypeDef    Bst            ;  // BR;     //后支撑腿
  legNameTypeDef    Fsw            ;  // FR;     //前摆动腿
  legNameTypeDef    Bsw            ;  // BL;     //后摆动腿
  bool              is_touching[4] ;  // false;  //足底传感器接触标志量,下标为FL,FR,BL,BR
  double            R              ;  // 0;      //roll
  double            P              ;  // 0;      //pitch
  double            Y              ;  // 0;      //yaw
  double            dR             ;  // 0;      //roll'
  double            dP             ;  // 0;      //pitch'
  double            dY             ;  // 0;      //yaw'
  double            theta[4][3]    ;  // {{0,0,0},{0,0,0},{0,0,0},{0,0,0}}; //关节角度
  vect3TypeDef      toe[4]         ;  // {{0,0,0},{0,0,0},{0,0,0},{0,0,0}}; //足底坐标
  vect3TypeDef      dtoe[4]        ;  // {{0,0,0},{0,0,0},{0,0,0},{0,0,0}}; //足底速度
  vect3TypeDef      st_toe[2]      ;  // {{0,0,0},{0,0,0}};                 //前支撑足，后支撑足，足底坐标
  vect3TypeDef      sw_toe[2]      ;  // {{0,0,0},{0,0,0}};                 //前摆动足，后摆动足，足底坐标
  double            vx             ;  // 0;  //机器人实际x速度
  double            vy             ;  // 0;  //机器人实际y速度
  double            wz             ;  // 0;  //机器人实际z速度
  vect3TypeDef      Ftoe0          ;  // {0,0,0};
  vect3TypeDef      Fdtoe0         ;  // {0,0,0};
  vect3TypeDef      Btoe0          ;  // {0,0,0};
  vect3TypeDef      Bdtoe0         ;  // {0,0,0};
  
}robotTypeDef;
//-----------------------------------------------------------extern
extern robotTypeDef robot;

extern void robot_init           ();
extern void updateRobotState     ();
extern void robot_control        ();

#endif

